{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "gQZtf4ZqM8HL"
   },
   "source": [
    "<center>\n",
    "<h4>CDS 110, Lecture 3</h4>\n",
    "<font color=blue><h1>Python Tools for Analyzing Linear Systems</h1></font>\n",
    "<h3>Richard M. Murray, Winter 2024</h3>\n",
    "</center>\n",
    "\n",
    "[Open in Google Colab](https://colab.research.google.com/drive/164yYvB86c2EvEcIHpUPNXCroiN9nnTAa)\n",
    "\n",
    "In this lecture we describe tools in the Python Control Systems Toolbox ([python-control](https://python-control.org)) that can be used to analyze linear systems, including some of the options available to present the information in different ways.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "try:\n",
    "  import control as ct\n",
    "  print(\"python-control\", ct.__version__)\n",
    "except ImportError:\n",
    "  !pip install control\n",
    "  import control as ct"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {
    "id": "qMVGK15gNQw2"
   },
   "source": [
    "## Coupled mass spring system\n",
    "\n",
    "Consider the spring mass system below:\n",
    "\n",
    "<center><img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/springmass-coupled.png\" width=640></center>\n",
    "\n",
    "We wish to analyze the time and frequency response of this system using a variety of python-control functions for linear systems analysis.\n",
    "\n",
    "### System dynamics\n",
    "\n",
    "The dynamics of the system can be written as\n",
    "\n",
    "$$\n",
    "\\begin{aligned}\n",
    "  m \\ddot{q}_1 &= -2 k q_1 - c \\dot{q}_1 + k q_2, \\\\\n",
    "  m \\ddot{q}_2 &= k q_1 - 2 k q_2 - c \\dot{q}_2 + ku\n",
    "\\end{aligned}\n",
    "$$\n",
    "\n",
    "or in state space form:\n",
    "\n",
    "$$\n",
    "\\begin{aligned}\n",
    "  \\dfrac{dx}{dt} &= \\begin{bmatrix}\n",
    "    0 & 0 & 1 & 0 \\\\\n",
    "    0 & 0 & 0 & 1 \\\\[0.5ex]\n",
    "    -\\dfrac{2k}{m} & \\dfrac{k}{m} & -\\dfrac{c}{m} & 0 \\\\[0.5ex]\n",
    "    \\dfrac{k}{m} & -\\dfrac{2k}{m} & 0 & -\\dfrac{c}{m}\n",
    "  \\end{bmatrix} x\n",
    "  + \\begin{bmatrix}\n",
    "    0 \\\\ 0 \\\\[0.5ex] 0 \\\\[1ex] \\dfrac{k}{m}\n",
    "  \\end{bmatrix} u.\n",
    "\\end{aligned}\n",
    "$$\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define the parameters for the system\n",
    "m, c, k = 1, 0.1, 2\n",
    "# Create a linear system\n",
    "A = np.array([\n",
    "    [0, 0, 1, 0],\n",
    "    [0, 0, 0, 1],\n",
    "    [-2*k/m, k/m, -c/m, 0],\n",
    "    [k/m, -2*k/m, 0, -c/m]\n",
    "])\n",
    "B = np.array([[0], [0], [0], [k/m]])\n",
    "C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])\n",
    "D = 0\n",
    "\n",
    "sys = ct.ss(A, B, C, D, outputs=['q1', 'q2'], name=\"coupled spring mass\")\n",
    "print(sys)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "kobxJ1yG4v_1"
   },
   "source": [
    "Another way to get these same dynamics is to define an input/output system:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "coupled_params = {'m': 1, 'c': 0.1, 'k': 2}\n",
    "def coupled_update(t, x, u, params):\n",
    "  m, c, k = params['m'], params['c'], params['k']\n",
    "  return np.array([\n",
    "      x[2], x[3],\n",
    "      -2*k/m * x[0] + k/m * x[1] - c/m * x[2],\n",
    "      k/m * x[0] -2*k/m * x[1] - c/m * x[3] + k/m * u[0]\n",
    "  ])\n",
    "def coupled_output(t, x, u, params):\n",
    "  return x[0:2]\n",
    "coupled = ct.nlsys(\n",
    "    coupled_update, coupled_output, inputs=1, outputs=['q1', 'q2'],\n",
    "    states=['q1', 'q2', 'q1dot', 'q2dot'], name='coupled (nl)',\n",
    "    params=coupled_params\n",
    ")\n",
    "print(coupled.linearize([0, 0, 0, 0], [0]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "YmH87LEXWo1U"
   },
   "source": [
    "### Initial response\n",
    "\n",
    "The `initial_response` function can be used to compute the response of the system with no input, but starting from a given initial condition.  This function returns a response object, which can be used for plotting."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "response = ct.initial_response(sys, X0=[1, 0, 0, 0])\n",
    "cplt = response.plot()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "Y4aAxYvZRBnD"
   },
   "source": [
    "If you want to play around with the way the data are plotted, you can also use the response object to get direct access to the states and outputs."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot the outputs of the system on the same graph, in different colors\n",
    "t = response.time\n",
    "x = response.states\n",
    "plt.plot(t, x[0], 'b', t, x[1], 'r')\n",
    "plt.legend(['$x_1$', '$x_2$'])\n",
    "plt.xlim(0, 50)\n",
    "plt.ylabel('States')\n",
    "plt.xlabel('Time [s]')\n",
    "plt.title(\"Initial response from $x_1 = 1$, $x_2 = 0$\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "Cou0QVnkTou9"
   },
   "source": [
    "There are also lots of options available in `initial_response` and `.plot()` for tuning the plots that you get."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for X0 in [[1, 0, 0, 0], [0, 2, 0, 0], [1, 2, 0, 0], [0, 0, 1, 0], [0, 0, 2, 0]]:\n",
    "  response = ct.initial_response(sys, T=20, X0=X0)\n",
    "  response.plot(label=f\"{X0=}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "b3VFPUBKT4bh"
   },
   "source": [
    "### Step response\n",
    "\n",
    "Similar to `initial_response`, you can also generate a step response for a linear system using the `step_response` function, which returns a time  response object:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cplt = ct.step_response(sys).plot()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "iHZR1Q3IcrFT"
   },
   "source": [
    "We can analyze the properties of the step response using the `stepinfo` command:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "step_info = ct.step_info(sys)\n",
    "print(\"Input 0, output 0 rise time = \",\n",
    "      step_info[0][0]['RiseTime'], \"seconds\\n\")\n",
    "step_info"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "F8KxXwqHWFab"
   },
   "source": [
    "Note that by default the inputs are not included in the step response plot (since they are a bit boring), but you can change that:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "stepresp = ct.step_response(sys)\n",
    "cplt = stepresp.plot(plot_inputs=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot the inputs on top of the outputs\n",
    "cplt = stepresp.plot(plot_inputs='overlay')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Look at the \"shape\" of the step response\n",
    "print(f\"{stepresp.time.shape=}\")\n",
    "print(f\"{stepresp.inputs.shape=}\")\n",
    "print(f\"{stepresp.states.shape=}\")\n",
    "print(f\"{stepresp.outputs.shape=}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "FDfZkyk1ly0T"
   },
   "source": [
    "## Forced response\n",
    "\n",
    "To compute the response to an input, using the convolution equation, we can use the `forced_response` function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "T = np.linspace(0, 50, 500)\n",
    "U1 = np.cos(T)\n",
    "U2 = np.sin(3 * T)\n",
    "\n",
    "resp1 = ct.forced_response(sys, T, U1)\n",
    "resp2 = ct.forced_response(sys, T, U2)\n",
    "resp3 = ct.forced_response(sys, T, U1 + U2)\n",
    "\n",
    "# Plot the individual responses\n",
    "resp1.sysname = 'U1'; resp1.plot(color='b')\n",
    "resp2.sysname = 'U2'; resp2.plot(color='g')\n",
    "resp3.sysname = 'U1 + U2'; resp3.plot(color='r');"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Show that the system response is linear\n",
    "cplt = resp3.plot()\n",
    "cplt.axes[0, 0].plot(resp1.time, resp1.outputs[0] + resp2.outputs[0], 'k--')\n",
    "cplt.axes[1, 0].plot(resp1.time, resp1.outputs[1] + resp2.outputs[1], 'k--')\n",
    "cplt.axes[2, 0].plot(resp1.time, resp1.inputs[0] + resp2.inputs[0], 'k--');"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Show that the forced response from non-zero initial condition is not linear\n",
    "X0 = [1, 0, 0, 0]\n",
    "resp1 = ct.forced_response(sys, T, U1, X0=X0)\n",
    "resp2 = ct.forced_response(sys, T, U2, X0=X0)\n",
    "resp3 = ct.forced_response(sys, T, U1 + U2, X0=X0)\n",
    "\n",
    "cplt = resp3.plot()\n",
    "cplt.axes[0, 0].plot(resp1.time, resp1.outputs[0] + resp2.outputs[0], 'k--')\n",
    "cplt.axes[1, 0].plot(resp1.time, resp1.outputs[1] + resp2.outputs[1], 'k--')\n",
    "cplt.axes[2, 0].plot(resp1.time, resp1.inputs[0] + resp2.inputs[0], 'k--');"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "mo7hpvPQkKke"
   },
   "source": [
    "### Frequency response"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Manual computation of the frequency response\n",
    "resp = ct.input_output_response(sys, T, np.sin(1.35 * T))\n",
    "\n",
    "cplt = resp.plot(\n",
    "    plot_inputs='overlay', \n",
    "    legend_map=np.array([['lower left'], ['lower left']]),\n",
    "    label=[['q1', 'u[0]'], ['q2', None]])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "muqeLlJJ6s8F"
   },
   "source": [
    "The magnitude and phase of the frequency response is controlled by the transfer function,\n",
    "\n",
    "$$\n",
    "G(s) = C (sI - A)^{-1} B + D\n",
    "$$\n",
    "\n",
    "which can be computed using the `ss2tf` function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    G = ct.ss2tf(sys, name='u to q1, q2')\n",
    "except ct.ControlMIMONotImplemented:\n",
    "    # Create SISO transfer functions, in case we don't have slycot\n",
    "    G = ct.ss2tf(sys[0, 0], name='u to q1')\n",
    "print(G)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Gain and phase for the simulation above\n",
    "from math import pi\n",
    "val = G(1.35j)\n",
    "print(f\"{G(1.35j)=}\")\n",
    "print(f\"Gain: {np.absolute(val)}\")\n",
    "print(f\"Phase: {np.angle(val)}\", \" (\", np.angle(val) * 180/pi, \"deg)\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Gain and phase at s = 0 (= steady state step response)\n",
    "print(f\"{G(0)=}\")\n",
    "print(\"Final value of step response:\", stepresp.outputs[0, 0, -1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "I9eFoXm92Jgj"
   },
   "source": [
    "The frequency response across all frequencies can be computed using the `frequency_response` function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "freqresp = ct.frequency_response(sys)\n",
    "cplt = freqresp.plot()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "pylQb07G2cqe"
   },
   "source": [
    "By default, frequency responses are plotted using a \"Bode plot\", which plots the log of the magnitude and the (linear) phase against the log of the forcing frequency.\n",
    "\n",
    "You can also call the Bode plot command directly, and change the way the data are presented:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cplt = ct.bode_plot(sys, overlay_outputs=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "I_LTjP2J6gqx"
   },
   "source": [
    "Note the \"dip\" in the frequency response for y[1] at frequency 2 rad/sec, which corresponds to a \"zero\" of the transfer function.\n",
    "\n",
    "This dip becomes even more pronounced in the case of low damping coefficient $c$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cplt = ct.frequency_response(\n",
    "    coupled.linearize([0, 0, 0, 0], [0], params={'c': 0.01})\n",
    ").plot(overlay_outputs=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "c7eWm8LCGh01"
   },
   "source": [
    "## Additional resources\n",
    "* [Code for FBS2e figures](https://fbswiki.org/wiki/index.php/Category:Figures): Python code used to generate figures in FBS2e\n",
    "* [Python-control documentation for plotting time responses](https://python-control.readthedocs.io/en/0.10.0/plotting.html#time-response-data)\n",
    "* [Python-control documentation for plotting frequency responses](https://python-control.readthedocs.io/en/0.10.0/plotting.html#frequency-response-data)\n",
    "* [Python-control examples](https://python-control.readthedocs.io/en/0.10.0/examples.html): lots of Python and Jupyter examples of control system analysis and design\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
